package simplescenegraph.math;

public class AngleAxis3f {

	private float a;
	
	private Vector3f u;
	
	public AngleAxis3f() {
		a = 0;
		u = new Vector3f();
	}
	
	public AngleAxis3f(float a0, Vector3f u0) {
		a = a0;
		u = new Vector3f(u0);
	}
	
	public AngleAxis3f(Matrix3f m0) {
		
		a = (float) Math.acos( (m0.trace() - 1) / 2.0f );
		
		if (a != Math.PI) {
			u = new Vector3f(m0.m[7] - m0.m[5], m0.m[2] - m0.m[6], m0.m[3] - m0.m[1]);
		}
		else {
			// verifying which component of the axis, from the diagonal entries of 
			// the rotation matrix is the highest
			// if it's m11...
			u = new Vector3f();
			if (m0.m[0] > m0.m[4] && m0.m[0] > m0.m[8]) {
				u.x = (float) Math.sqrt(m0.m[0] - m0.m[4] - m0.m[8]) / 2.0f;
				u.y = m0.m[1] / (2 * u.x);
				u.z = m0.m[2] / (2 * u.x);
			}
			// if it's m22...
			else if (m0.m[4] > m0.m[0] && m0.m[4] > m0.m[8]) {
				u.y = (float) Math.sqrt(m0.m[4] - m0.m[0] - m0.m[8]) / 2.0f;
				u.x = m0.m[1] / (2 * u.y);
				u.z = m0.m[2] / (2 * u.y);				
			}
			// if it's m33
			else {
				u.z = (float) Math.sqrt(m0.m[8] - m0.m[0] - m0.m[4]) / 2.0f;
				u.x = m0.m[1] / (2 * u.z);
				u.y = m0.m[2] / (2 * u.z);					
			}
		}
		u.normalize();
		
	}
	
	public float getAngle() {
		return a;
	}
	
	public Vector3f getAxis() {
		return u;
	}
	
}
